/*
  Program Name:  MC_Slave
  Project:       LLH Hopscotch
  Written By:    Daniel Lee
  Revision:      0.4
*/

#include<SoftwareSerial.h>

// Pins 2 and 7 are reserved for FTDI comms
// Pins 0 and 1 are reserved for USB comms (during programming only)

#define TRXRATE 19200
#define TERMBAUD 9600
#define LEDPIN 3
#define SENSORPIN A5
#define SPKRPIN 11
#define RX_1 4
#define TX_1 5
#define RX_2 6
#define TX_2 8
#define RX_3 9
#define TX_3 10
#define RX_4 12
#define TX_4 13
#define MAX_BUFFER_SIZE 64
#define TIME_OUT 25
#define NO_RESPONSE 160
//#define MSG_RCVD 102

//STATE CODES:
#define LISTENING 0
#define CHANNEL_SET 1
#define PROCESS_MSG 2

//COMMAND CODES:
#define COM 170
#define ROGER 171
#define NEGATIVE 172
#define SETID 173
#define SENDID 174
#define SETNEIGHBORS 175
#define IDSET 176
#define NEIGHBORSSET 177
#define NOCOM 255


SoftwareSerial mySerial[4] = {SoftwareSerial(RX_1,TX_1),SoftwareSerial(RX_2,TX_2),SoftwareSerial(RX_3,TX_3),SoftwareSerial(RX_4,TX_4)};

int active_channel;
int listen_count;
int state;
uint8_t read_buf[MAX_BUFFER_SIZE];
uint8_t write_buf[MAX_BUFFER_SIZE];
int read_buf_len, write_buf_len;
long waitTime;
uint8_t myID;
int next_available_ID;
//uint8_t cur_read;
long prevTime;
long startTime;
int ledState = LOW;
int port_map[4];
int return_path[16];
int return_path_len;



void blink_led(int ledPin){
  long blinkTime = 250;
  long curTime = millis();
  if((curTime - prevTime) > blinkTime){
    prevTime = curTime;
    if(ledState == LOW){
      ledState = HIGH;
    }else{
      ledState = LOW;
    }
  }
  digitalWrite(ledPin, ledState);
}

int isvalueinarray(int val, int *arr, int size){
    int i;
    for (i=0; i < size; i++) {
        if (arr[i] == val)
            return i;
    }
    return -1;
}

//Write message buffer to serial TX
void writebuffertoserial(int channel, uint8_t *arr, int buf_size) {
  int i;
  int temp_len = buf_size;
  uint8_t temp_buf[buf_size+1];
  const uint8_t start_sig = COM;
  while(temp_len > 0){
    temp_buf[temp_len] = arr[temp_len-1];
    temp_len--;
  }
  temp_buf[0] = start_sig;
  Serial.print("Writing");
  for(i=0;i<buf_size+1;i++){
    Serial.print(", ");
    Serial.print(temp_buf[i], DEC);
  }
  Serial.print(" to serial channel ");
  Serial.println(channel, DEC);
  if(channel > 0 && channel < 5){
    mySerial[channel-1].write(temp_buf, buf_size+1);
  }else{
    Serial.println("Invalid channel selection for writebuffertoserial()");
    Serial.print("Error! cannot write to serial channel ");
    Serial.println(channel);
  }
}

/*int getChannel(){
    
    return active_channel;
}*/

int setChannel(int channel){
    if(channel > 0 && channel < 5){
        mySerial[channel-1].listen();
        delay(100);
        active_channel = channel;
        return 0;
    }else{
        return -1;
    }
}

void set_ID(){
  int send_status = -1;
  /*for(int i=0;i<read_buf_len;i++){
    Serial.print(read_buf[i]);
  }
  Serial.println("");*/
  myID = read_buf[1];
  write_buf[0] = IDSET;
  write_buf[1] = myID;
  write_buf_len = 2;
  send_status = send_msg();
  if(send_status != 0){
    Serial.print("Error: status code = ");
    Serial.print(send_status);
    Serial.println(". Something went wrong with board-to-board comms!!!");
  }else{
    state = LISTENING;
  }
}

void set_neighbors(){
  int send_status = -1;
  next_available_ID = read_buf[1];
  for(int j=2;j<read_buf_len;j++){
    return_path[j-2] = read_buf[j];
    Serial.println(return_path[return_path_len]);
    return_path_len++;
  }
  for(int i=0;i<4;i++){
    setChannel(i+1);
    port_map[i] = get_ID();
    if(port_map[i] == NO_RESPONSE){
      port_map[i] = -1;
    }else if(port_map[i] == 255){
      port_map[i] = send_set_ID();
      if(port_map[i] >= next_available_ID){
        next_available_ID = port_map[i] + 1;
      }
    }
  }
  //find port with ID '0'
  int port_num = 0; //invalid port number
  for(int i=0;i<4;i++){
    if(port_map[i] == return_path[0]){
      port_num = i+1;
    }
  }
  if(port_num == 0){
    Serial.println("no valid port found!");
  }else{
    setChannel(port_num);
  }
  write_buf_len = 0;
  for(int i=1;i<return_path_len;i++){
    write_buf[write_buf_len] = return_path[i];
    write_buf_len++;
  }
  write_buf[write_buf_len] = NEIGHBORSSET;
  write_buf_len++;
  write_buf[write_buf_len] = myID;
  write_buf_len++;
  write_buf[write_buf_len] = port_map[0];
  write_buf_len++;
  write_buf[write_buf_len] = port_map[1];
  write_buf_len++;
  write_buf[write_buf_len] = port_map[2];
  write_buf_len++;
  write_buf[write_buf_len] = port_map[3];
  write_buf_len++;
  write_buf[write_buf_len] = next_available_ID;
  write_buf_len++;
  
  send_status = send_msg();
  if(send_status != 0){
    Serial.print("Error: status code = ");
    Serial.print(send_status);
    Serial.println(". Something went wrong with board-to-board comms!!!");
  }else{
    state = LISTENING;
  }
}

int send_set_ID(){
  int send_status = -1;
  write_buf[0] = SETID;
  write_buf[1] = next_available_ID;
  write_buf_len = 2;
  send_status = send_msg();
  if(send_status != 0){
    //something went wrong with board to board comms
    Serial.print("Error: status code = ");
    Serial.print(send_status);
    Serial.println(". Something went wrong with board-to-board comms!!!");
    return -1;
  }else{
    //wait for confirmation
    int recvd = 0;
    while(recvd == 0){
      delay(1);
      if(mySerial[active_channel-1].read() == COM){
        Serial.println("COM");
        //uint8_t cur_read = -1;
        int cnt = 0;
        while(mySerial[active_channel-1].available() > 0 && cnt < MAX_BUFFER_SIZE && mySerial[active_channel-1].peek() != COM){
          read_buf[cnt] = mySerial[active_channel-1].read();
          cnt++;
          Serial.println("available");
          delay(1);
        }
        read_buf_len = cnt;
      }    
      if(read_buf_len > 0 && read_buf[0] == IDSET){
        send_msg_received();
        recvd = 1;
      }
    }
    return read_buf[1];
  }
}

int get_ID(){
  int send_status = -1;
  write_buf[0] = SENDID;
  write_buf_len = 1;
  send_status = send_msg();
  if(send_status != 0){
  //something went wrong with board to board comms
    Serial.print("Error: status code = ");
    Serial.print(send_status);
    Serial.println(". Something went wrong with board-to-board comms!!!");
    return NO_RESPONSE;
  }else{
    //wait for response
    int recvd = 0;
    while(recvd == 0){
      delay(1);
      if(mySerial[active_channel-1].read() == COM){
        Serial.println("COM");
        //uint8_t cur_read = -1;
        int cnt = 0;
        while(mySerial[active_channel-1].available() > 0 && cnt < MAX_BUFFER_SIZE && mySerial[active_channel-1].peek() != COM){
          read_buf[cnt] = mySerial[active_channel-1].read();
          cnt++;
          Serial.println("available");
          delay(1);
        }
        read_buf_len = cnt;
      }    
      if(read_buf_len > 0){
        send_msg_received();
        recvd = 1;
      }
    }
    return read_buf[0];
  }
}

void send_ID(){
    int send_status = -1;
    write_buf[0] = myID;
    write_buf_len = 1;
    send_status = send_msg();
    if(send_status != 0){
        //something went wrong with board to board comms
        Serial.print("Error: status code = ");
        Serial.print(send_status);
        Serial.println(". Something went wrong with board-to-board comms!!!");
    }else{
      state = LISTENING;
    }
    //writebuffertoserial(active_channel, write_buf, write_buf_len);
}

int send_msg(){
  int count = 0;
  int confirm_rcvd = 0;
  int temp_buf_len = 0;
  uint8_t temp_buf[MAX_BUFFER_SIZE];
  while(mySerial[active_channel-1].available() > 0)mySerial[active_channel-1].read();
  while(confirm_rcvd != 1 && count < TIME_OUT){
    writebuffertoserial(active_channel, write_buf, write_buf_len);
    delay(30);
    if(mySerial[active_channel-1].read() == COM){
        int cnt = 0;
        while(mySerial[active_channel-1].available() > 0 && mySerial[active_channel-1].peek() != COM){
            temp_buf[cnt] = mySerial[active_channel-1].read();
            cnt++;
        }
        temp_buf_len = cnt;
        if(cnt > 0 && temp_buf[cnt-1] == ROGER){
          confirm_rcvd = 1;
          Serial.println("...");
        }else{
          while(mySerial[active_channel-1].available() > 0)mySerial[active_channel].read();
        }
    }else{
        //if the input buffer doesn't start with '170', flush the garbage.
        while(mySerial[active_channel-1].available() > 0)mySerial[active_channel-1].read();
    }
    count++;
  }
  delay(60);
  while(mySerial[active_channel-1].available() > 0)mySerial[active_channel-1].read();
  read_buf_len = 0;
  
  if(count > TIME_OUT || temp_buf_len <= 0){
    return -1;
  }else{
    return 0;
  }
}

void send_msg_received(){
    uint8_t msg_rcvd = ROGER;
    for(int i=0;i<2;i++){
        writebuffertoserial(active_channel, &msg_rcvd, 1);
        delayMicroseconds(500);
    }
    //returning with the assumption that our confirmation was received
}

void setup(){
  Serial.begin(TERMBAUD);
  Serial.println("attempting to initialize Serial comms...");
  Serial.println("...");
  pinMode(RX_1, INPUT_PULLUP);
  pinMode(TX_1, OUTPUT);
  pinMode(RX_2, INPUT_PULLUP);
  pinMode(TX_2, OUTPUT);
  pinMode(RX_3, INPUT_PULLUP);
  pinMode(TX_3, OUTPUT);
  pinMode(RX_4, INPUT_PULLUP);
  pinMode(TX_4, OUTPUT);
  for(int i=3;i>=0;i--){
    mySerial[i].begin(TRXRATE);
    Serial.println("...");
  }
  pinMode(LEDPIN, OUTPUT);
  Serial.println("...initialization complete.");
  //Serial.println("Generating random ID...");
  myID = -1;
  active_channel = 1;
  listen_count = 1;
  digitalWrite(LEDPIN, HIGH);
  next_available_ID = -1;
  //cur_read = -1;
  waitTime = 20;
  read_buf_len = write_buf_len = 0;
  for(int i=0;i<4;i++){
    port_map[i] = -1;
  }
  for(int i=0;i<16;i++){
    return_path[i] = -1;
  }
  return_path_len = 0;
  state = LISTENING;
  prevTime = millis();
}

void loop(){
  
  switch(state){
    case LISTENING:
        //in listening mode, cycle through all ports until info is received
        setChannel(listen_count);
        Serial.println(active_channel,DEC);
        startTime = millis();
        while(mySerial[active_channel-1].available() == 0 && ((millis() - startTime) < waitTime)){}
        if(mySerial[active_channel-1].available() > 0){
            state = CHANNEL_SET;
            Serial.println("mode: CHANNEL_SET");
        }else{
            listen_count++;
            delay(3);
        }
        if(listen_count > 4){
            listen_count = 1;
        }
        break;
    case CHANNEL_SET:
        Serial.println(active_channel);
        Serial.println(mySerial[active_channel-1].peek(),DEC);
        //delay(1);  // will need this delay when the Serial.println()'s are removed. As they are acting as the needed delay.
        if(mySerial[active_channel-1].read() == COM){
            int cnt=0;
            //uint8_t cur_read = -1;
            while(mySerial[active_channel-1].available() > 0 && cnt < MAX_BUFFER_SIZE && (uint8_t)mySerial[active_channel-1].peek() != COM){
                //cur_read = 
                read_buf[cnt] = mySerial[active_channel-1].read(); //cur_read;
                cnt++;
                delay(1);
            }
            read_buf_len = cnt;
            send_msg_received();
            state = PROCESS_MSG;
            Serial.println("mode: PROCESS_MSG");
        }else{
            while(mySerial[active_channel-1].available() > 0)mySerial[active_channel-1].read();
        }
        break;
    case PROCESS_MSG:
        //once the program reaches this point, it has a message
        //consisting of uint8_t values in the read_buf array
        //according to the COMMAND CODE scheme outlined above
        //if the first value in the read_buf array is greater than
        //170, then the message has reached its destination and
        //this COMMAND CODE should be executed
        
        if(read_buf[0] > ROGER){
            //execute COMMAND CODE
            switch(read_buf[0]){
                case SENDID:
                    send_ID();
                    break;
                case SETID:
                    set_ID();
                    break;
                case SETNEIGHBORS:
                    set_neighbors();
                    break;
                default:
                    state = LISTENING;
                    break;
            }
        }else{
            //pass the message onto the next board
            int send_status = -1;
            int port = isvalueinarray(read_buf[0], port_map, 4) + 1;
            setChannel(port);
            for(int i=0;i<read_buf_len;i++){
              write_buf[i] = read_buf[i+1];
              write_buf_len = read_buf_len-1;
            }
            send_status = send_msg();
            if(send_status != 0){
              //something went wrong with board to board comms
              Serial.print("Error: status code = ");
              Serial.print(send_status);
              Serial.println(". Something went wrong with board-to-board comms!!!");
            }else{
              state = LISTENING;
            }
        }
        break;
    case SETNEIGHBORS:
    default:
        break;
  }
}
